#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
云台控制模块
Gimbal Control Module

封装舵机控制逻辑，提供统一的云台控制接口
"""

import logging
import time
import threading
from typing import Tuple, Optional, Dict, Any

from ..config.settings import Settings, GimbalConfig
from .gimbal import ServoController
from .tracking import CoordinateCalculator
from .exceptions import GimbalAngleOutOfRangeError, GimbalNotConnectedError, GimbalMoveError

logger = logging.getLogger("GimbalControl")


class GimbalController:
    """
    云台控制器类
    
    封装原有的ServoController类，提供统一的云台控制接口
    """
    
    def __init__(self, settings: Settings):
        """
        初始化云台控制器
        
        Args:
            settings: 全局配置对象
        """
        self.settings = settings
        self.config: GimbalConfig = settings.gimbal
        self.connected = False
        self.lock = threading.Lock()
        
        # 当前角度
        self.current_pan = self.config.init_pan_angle
        self.current_tilt = self.config.init_tilt_angle
        
        # 控制统计
        self.control_stats = {
            'total_commands': 0,
            'successful_commands': 0,
            'failed_commands': 0,
            'last_command_time': 0.0,
            'last_pan_angle': self.config.init_pan_angle,
            'last_tilt_angle': self.config.init_tilt_angle
        }
        
        # 初始化原有控制器
        self._init_controller()
        self.auto_tracking_enabled = False
        
        logger.info("云台控制器初始化完成")
        logger.info("串口: %s, 波特率: %d", self.config.serial_port, self.config.baudrate)
        logger.info("初始角度: Pan=%.1f°, Tilt=%.1f°", self.config.init_pan_angle, self.config.init_tilt_angle)
    
    def _init_controller(self):
        """初始化原有的控制器"""
        try:
            self.controller = ServoController(
                gimbal_config=self.settings.gimbal,
                serial_config=self.settings.serial,
                tracking_config=self.settings.tracking,
            )
            logger.info("舵机控制器初始化成功")
            
        except Exception as e:
            logger.error(f"云台控制器初始化失败: {e}")
            self.controller = None
            raise
    
    def connect(self) -> bool:
        """
        连接云台
        
        Returns:
            bool: 是否成功连接
        """
        if self.controller is None:
            logger.error("控制器未初始化")
            return False
        
        try:
            success = self.controller.connect()
            if success:
                self.connected = True
                self.current_pan = self.controller.current_pan
                self.current_tilt = self.controller.current_tilt
                logger.info("云台连接成功")
            else:
                self.connected = False
                logger.error("云台连接失败")
            
            return success
            
        except Exception as e:
            logger.error(f"云台连接异常: {e}")
            self.connected = False
            return False
    
    def disconnect(self):
        """断开云台连接"""
        if self.controller:
            try:
                self.controller.disconnect()
                self.connected = False
                logger.info("云台已断开连接")
            except Exception as e:
                logger.error(f"云台断开连接异常: {e}")
    
    def move_to_angle(self, pan: float, tilt: float) -> bool:
        """
        移动云台到指定角度

        Args:
            pan: 水平角度 (0-270度)
            tilt: 垂直角度 (0-270度)

        Returns:
            bool: 是否成功执行

        Raises:
            GimbalNotConnectedError: 云台未连接
            GimbalAngleOutOfRangeError: 角度超出范围
            GimbalMoveError: 云台移动失败
        """
        if not self.connected or self.controller is None:
            logger.warning("云台未连接")
            raise GimbalNotConnectedError()

        # 角度范围检查
        self._validate_angles(pan, tilt)
        
        with self.lock:
            try:
                start_time = time.time()
                
                # 移动水平舵机
                success_pan = self.controller.move_servos(self.config.pan_pin, pan)
                if not success_pan:
                    error_msg = f"水平舵机移动失败: {pan}°"
                    logger.error(error_msg)
                    self._update_stats(False, time.time() - start_time)
                    raise GimbalMoveError("pan", pan, "舵机硬件响应失败")

                time.sleep(0.1)  # 短暂延迟

                # 移动垂直舵机
                success_tilt = self.controller.move_servos(self.config.tilt_pin, tilt)
                if not success_tilt:
                    error_msg = f"垂直舵机移动失败: {tilt}°"
                    logger.error(error_msg)
                    self._update_stats(False, time.time() - start_time)
                    raise GimbalMoveError("tilt", tilt, "舵机硬件响应失败")
                
                # 更新当前角度
                self.current_pan = pan
                self.current_tilt = tilt
                
                # 更新统计
                execution_time = time.time() - start_time
                self._update_stats(True, execution_time)
                
                logger.debug(f"云台移动成功: Pan={pan}°, Tilt={tilt}°, 耗时={execution_time:.3f}s")
                return True
                
            except Exception as e:
                if isinstance(e, (GimbalAngleOutOfRangeError, GimbalNotConnectedError, GimbalMoveError)):
                    # 重新抛出我们的自定义异常
                    raise
                else:
                    # 包装其他未知异常
                    logger.error(f"云台移动异常: {e}")
                    self._update_stats(False, time.time() - start_time)
                    raise GimbalMoveError("unknown", 0, f"未知错误: {str(e)}")
    
    def move_relative(self, pan_delta: float, tilt_delta: float) -> bool:
        """
        相对移动云台

        Args:
            pan_delta: 水平角度变化量
            tilt_delta: 垂直角度变化量

        Returns:
            bool: 是否成功执行

        Raises:
            GimbalNotConnectedError: 云台未连接
            GimbalAngleOutOfRangeError: 角度超出范围
            GimbalMoveError: 云台移动失败
        """
        new_pan = self.current_pan + pan_delta
        new_tilt = self.current_tilt + tilt_delta

        return self.move_to_angle(new_pan, new_tilt)
    
    def reset_position(self) -> bool:
        """
        重置云台到初始位置
        
        Returns:
            bool: 是否成功执行
        """
        return self.move_to_angle(self.config.init_pan_angle, self.config.init_tilt_angle)
    
    def get_current_angles(self) -> Tuple[float, float]:
        """
        获取当前角度
        
        Returns:
            Tuple[float, float]: (水平角度, 垂直角度)
        """
        return self.current_pan, self.current_tilt
    
    def get_status(self) -> Dict[str, Any]:
        """
        获取云台状态
        
        Returns:
            Dict[str, Any]: 状态信息
        """
        return {
            'connected': self.connected,
            'current_angles': {
                'pan': self.current_pan,
                'tilt': self.current_tilt
            },
            'config': {
                'serial_port': self.config.serial_port,
                'baudrate': self.config.baudrate,
                'pan_pin': self.config.pan_pin,
                'tilt_pin': self.config.tilt_pin,
                'angle_range': self.config.angle_range
            },
            'stats': self.control_stats.copy()
        }
    
    def start_auto_tracking(self, coord_calculator=None) -> bool:
        """
        启动自动追踪

        Args:
            coord_calculator: 坐标计算器实例（可选）

        Returns:
            bool: 是否成功启动
        """
        if not self.connected or self.controller is None:
            logger.warning("云台未连接，无法启动自动追踪")
            return False

        try:
            # 如果没有提供坐标计算器，创建一个默认的
            if coord_calculator is None:
                camera_cfg = self.settings.camera
                coord_calculator = CoordinateCalculator(
                    frame_width=camera_cfg.width,
                    frame_height=camera_cfg.height,
                    tracking_config=self.settings.tracking,
                )

            # 启动原有控制器的自动追踪
            self.controller.start_auto_tracking(coord_calculator)
            self.auto_tracking_enabled = True
            logger.info("自动追踪已启动")
            return True
        except Exception as e:
            logger.error(f"启动自动追踪失败: {e}")
            return False
    
    def stop_auto_tracking(self) -> bool:
        """
        停止自动追踪

        Returns:
            bool: 是否成功停止
        """
        if self.controller is None:
            return False

        try:
            self.controller.stop_auto_tracking()
            self.auto_tracking_enabled = False
            logger.info("自动追踪已停止")
            return True
        except Exception as e:
            logger.error(f"停止自动追踪失败: {e}")
            return False
    
    def update_tracking_target(self, target_x: int, target_y: int, frame_width: int, frame_height: int) -> bool:
        """
        更新追踪目标
        
        Args:
            target_x: 目标X坐标
            target_y: 目标Y坐标
            frame_width: 画面宽度
            frame_height: 画面高度
            
        Returns:
            bool: 是否成功更新
        """
        if not self.connected or self.controller is None:
            return False
        
        try:
            self.controller.update_tracking_target(target_x, target_y, frame_width, frame_height)
            return True
        except Exception as e:
            logger.error(f"更新追踪目标失败: {e}")
            return False
    
    def _validate_angles(self, pan: float, tilt: float):
        """
        验证角度范围

        Args:
            pan: 水平角度
            tilt: 垂直角度

        Raises:
            GimbalAngleOutOfRangeError: 角度超出范围时抛出异常
        """
        min_angle, max_angle = self.config.angle_range

        if not (min_angle <= pan <= max_angle):
            logger.error(f"水平角度超出范围: {pan}° (范围: {min_angle}-{max_angle}°)")
            raise GimbalAngleOutOfRangeError("pan", pan, min_angle, max_angle)

        if not (min_angle <= tilt <= max_angle):
            logger.error(f"垂直角度超出范围: {tilt}° (范围: {min_angle}-{max_angle}°)")
            raise GimbalAngleOutOfRangeError("tilt", tilt, min_angle, max_angle)
    
    def _update_stats(self, success: bool, execution_time: float):
        """更新控制统计"""
        self.control_stats['total_commands'] += 1
        if success:
            self.control_stats['successful_commands'] += 1
        else:
            self.control_stats['failed_commands'] += 1
        
        self.control_stats['last_command_time'] = execution_time
        self.control_stats['last_pan_angle'] = self.current_pan
        self.control_stats['last_tilt_angle'] = self.current_tilt
